Physics Engines Continued
Physics Engines Continued
A while ago I created a "cookbook" for physics engines. About halfway through I
ran into a small problem: It's hard to display tons of 3D graphics code together in
a Jupyter notebook that doesn't support 3D graphics.
I have swapped entirely off of Python, and onto Processing for these notes now.
I choose Processing because the 3D graphics are very simple to implement, and
the language (Java) is a lowest-common-denominator language. Anything you can
do in Java is easily done in other languages. This is the same reason I had chosen
Python before- but in hindsight Java more resembles the sorts of languages you would
actually want to build a physics engine in, and was a better choice from the start.
I have created a quick review of what I covered in the previous notes, but it was really
more of an excuse for me to jog my own memory, and is incomplete.
On that note please begin by reading the first
set of notes before starting these ones.
Review Session
Time for a quick review of what we did so far. We had come up with a "particle physics" simulator, which could only simulate point-masses.
Those point masses could not have collisions with one another, and they could not have springs attached to them that are too stiff. All in
all it is a relatively limited physics simulator, but it would work to do your highschool homework. We could simulate gravity, drag, magnets,
buoyancy, and a wide range of other slow-acting forces, but if a force acted too quickly then it we could get an imprecision in the time frame
it was done at. These imprecisions would built up, and cause our physics simulator to go crazy.
I have gone through the effort of re-writing that point-mass/force-aggregate physics simulator, and it can be found below. I suggest
leafing through it to see what is different, and to refresh your memory.
Particle p;
void setup() {
size(800, 600, P3D);
frameRate(120);
lights();
fill(255);
stroke(125);
p = new Particle(new PVector(0, 0, 0), new PVector(10, 0, 0), 1.0);
p.registerForce(new Gravity());
p.registerForce(new AnchoredSpring(new PVector(0, 0, 0), 1.0, 1));
p.registerForce(new Drag(0.1, 0.1));
}
void draw() {
background(173, 216, 230);
pushMatrix();
translate(p.pos.x, p.pos.y, p.pos.z);
sphere(0.2);
popMatrix();
stroke(125);
line(p.pos.x, p.pos.y, p.pos.z, 0, 0, 0);
box(1);
p.tick(1.0/120);
assert(p.pos.z == 0 && p.vel.z == 0);
}
interface ForceGenerator {
PVector calculateForce(Particle p);
}
class Gravity implements ForceGenerator {
PVector calculateForce(Particle p) {
return new PVector(0, 2/p.inverseMass, 0);
}
}
class Drag implements ForceGenerator {
float linearCoefficient = 0.1;
float quadraticCoefficient = 0.01;
public Drag(float l, float q) {
this.linearCoefficient = l;
this.quadraticCoefficient = q;
}
PVector calculateForce(Particle p) {
float speed = p.vel.mag();
float dragCoefficient = linearCoefficient*speed + quadraticCoefficient*speed*speed;
PVector n = new PVector(p.vel.x, p.vel.y, p.vel.z);
n.normalize();
return n.mult(-dragCoefficient);
}
}
class AnchoredSpring implements ForceGenerator {
PVector anchorPoint;
float springConstant;
float restingLength;
public AnchoredSpring(PVector anchorPoint, float springConstant, float restingLength) {
this.anchorPoint = anchorPoint;
this.springConstant = springConstant;
this.restingLength = restingLength;
}
PVector calculateForce(Particle p) {
PVector deltaP = PVector.sub(p.pos, anchorPoint);
float dist = deltaP.mag();
float forceMagnitude = abs(dist - restingLength) * springConstant;
deltaP.normalize();
return PVector.mult(deltaP, -forceMagnitude);
}
}
class Spring implements ForceGenerator {
float springConstant;
float restingLength;
Particle p1;
Particle p2;
public Spring(Particle p1, Particle p2, float restingLength, float springConstant) {
this.p1 = p1;
this.p2 = p2;
this.restingLength = restingLength;
this.springConstant = springConstant;
}
PVector calculateForce(Particle p) {
Particle otherParticle = p1;
if (p == p1) otherParticle = p2;
if (p == p2) otherParticle = p1;
PVector deltaP = PVector.sub(p.pos, otherParticle.pos);
float dist = deltaP.mag();
float forceMagnitude = abs(dist - restingLength) * springConstant;
deltaP.normalize();
return PVector.mult(deltaP, -forceMagnitude);
}
}
class Particle {
PVector pos;
PVector vel;
float inverseMass;
ArrayList forceRegistry;
public Particle(PVector pos, PVector vel, float mass) {
this.pos = pos;
this.vel = vel;
this.inverseMass = 1.0/mass;
this.forceRegistry = new ArrayList();
}
void registerForce(ForceGenerator fgen) {
this.forceRegistry.add(fgen);
}
void tick(float dt) {
// Accumulate the effect of all the forces
PVector fnet = new PVector(0, 0, 0);
for (ForceGenerator fg : this.forceRegistry) {
fnet.add( fg.calculateForce(this));
}
// Then since F=MA, A = F/M
PVector acceleration = PVector.mult(fnet, inverseMass);
// Finally we do a little frame-based integration
this.vel.add( PVector.mult(acceleration, dt));
this.pos.add( PVector.mult(this.vel, dt));
}
}
This brings us back up to speed with page 124.
Collisions
this is a 3D simulation, but GIFs are 2D and the lighting is bad
Where we last left of we were entering the world of collisions, by working on a 3D program which identifies and corrects
collisions between rigid spheres. Let's go over how that works again before writing any code. Our collision resolution
system has 3 parts to it:
- Collision Detection which of course, detects when objects collide (or even touch each other).
- Velocity Resolution which decides on the speeds for objects post-collision.
- Interpentration Resolution which helps us deal with the imprecisions of our program.
Because we're working in discrete time-steps, its possible for objects to be inside one another.
Interpenetration resolution is the process of pulling them apart.
ArrayList particles;
void setup() {
size(800, 600, P3D);
frameRate(120);
lights();
fill(255);
stroke(125);
// Add a row of 10 or so particles, all experiencing gravity
particles = new ArrayList();
for (int i=0; i < 10; i++) {
Particle p = new Particle(
new PVector(i*2, 0, 0),
new PVector(0, 0, 0),
10
);
p.registerForce(new Gravity());
particles.add(p);
}
// We'll add one gravity-free particle for the others to bounce off of
// Like pochinko pinchinko palchinko the y'know with the the balls and uh uhmm
// LETS GO GAMBLING!!!
Particle p = new Particle(
new PVector(7.5, 15, 0),
new PVector(0, 0, 0),
10000
);
p.radius = 5;
particles.add(p);
}
void draw() {
background(173, 216, 230);
for (Particle p : particles) {
pushMatrix();
translate(p.pos.x, p.pos.y, p.pos.z);
sphere(p.radius);
popMatrix();
p.tick(1.0/120);
}
resolveAllParticleContacts(particles, 1.0/120);
}
class Particle {
PVector pos;
PVector vel;
float inverseMass;
float radius = 1;
ArrayList forceRegistry;
public Particle(PVector pos, PVector vel, float mass) {
this.pos = pos;
this.vel = vel;
this.inverseMass = 1.0/mass;
this.forceRegistry = new ArrayList();
}
void registerForce(ForceGenerator fgen) {
this.forceRegistry.add(fgen);
}
PVector getAcceleration() {
// F_net = Sum(F_i);
PVector fnet = new PVector(0, 0, 0);
for (ForceGenerator fg : this.forceRegistry) {
fnet.add( fg.calculateForce(this));
}
// Then since F=MA, A = F/M
PVector acceleration = PVector.mult(fnet, inverseMass);
return acceleration;
}
void tick(float dt) {
// Then since F=MA, A = F/M
PVector acceleration = getAcceleration();
// Finally we do a little frame-based integration
this.vel.add( PVector.mult(acceleration, dt));
this.pos.add( PVector.mult(this.vel, dt));
}
}
interface Contact {
float calculateSeparatingVelocity();
void resolveVelocity(float dt);
void resolveInterpenetration(float dt);
void resolve(float dt);
}
class ParticleContact implements Contact {
Particle p1;
Particle p2;
float restitution;
PVector contactNormal;
public ParticleContact(Particle p1, Particle p2) {
this.p1 = p1;
this.p2 = p2;
this.contactNormal = PVector.sub(p1.pos, p2.pos);
this.contactNormal.normalize();
}
float calculateSeparatingVelocity() {
return PVector.dot(contactNormal, PVector.sub(p1.vel, p2.vel));
}
void resolveVelocity(float dt) {
// This part only works for spheres, the rest should be OK though
//PVector contactNormal = PVector.sub(p1.pos, p2.pos);
//contactNormal.normalize();
// Figure out how quickly the objects are moving apart
float separatingVelocity = calculateSeparatingVelocity();
// Make sure they're actually colliding, and not drifting apart on their own.
if (separatingVelocity > 0) return;
// Once they bounce off they'll leave with some amount of their original energy
// (but not all of it)
float newSeparatingVelocity = -separatingVelocity * restitution;
// This is part of resting objet detection. instead of just checking if the
// objects are moving towards eachother, we'll check if they're accelerating to
// eachother as well.
PVector accCausedVelocity = PVector.sub(p1.getAcceleration(), p2.getAcceleration());
// TODO: Check the order of operations here....
float accCausedSepVelocity = PVector.dot(accCausedVelocity, PVector.mult(contactNormal, dt));
// If we've got a closing velocity due to acceleration build-up,
// remove it from the new separating velocity.
if (accCausedSepVelocity < 0) {
newSeparatingVelocity += restitution*accCausedSepVelocity;
// BUt don't let the separating velocity become negative
if (newSeparatingVelocity <0) newSeparatingVelocity = 0;
}
float dV = newSeparatingVelocity - separatingVelocity;
// We'll split up the change in velocity depending on the mass of
// the particles
float totalInverseMass = p1.inverseMass + p2.inverseMass;
if (totalInverseMass <= 0) return;
float impulse = dV / totalInverseMass;
PVector impulsePerIMass = PVector.mult(contactNormal, impulse);
// Apply the impulses in the direction of contact, proportionally to the imass
p1.vel.add( PVector.mult(impulsePerIMass, p1.inverseMass));
p2.vel.add( PVector.mult(impulsePerIMass, -p2.inverseMass));
}
void resolveInterpenetration(float dt) {
// Euclidean distance between the centers of the two spheres
float penetration = PVector.sub(p1.pos, p2.pos).mag();
penetration -= (p1.radius + p2.radius);
// Make sure this point masses have mass
float totalIMass = p1.inverseMass + p2.inverseMass;
if (totalIMass <= 0) return;
// If they aren't interpenetrating then our job is easy.
if (penetration <=0) return;
// From each according to his displacement, to each according to his inverse mass
PVector movementPerIMass = PVector.mult(contactNormal, -penetration / totalIMass);
p1.pos.add( PVector.mult(movementPerIMass, p1.inverseMass));
p2.pos.add( PVector.mult(movementPerIMass, p2.inverseMass));
}
void resolve(float dt) {
this.resolveVelocity(dt);
this.resolveInterpenetration(dt);
}
}
ArrayList checkForParticleCollisions(ArrayList particles) {
ArrayList contacts = new ArrayList();
// For now, we'll do this with the naive O(n^2) method.
for (Particle p1 : particles) {
for (Particle p2: particles) {
float dist = PVector.sub(p1.pos, p2.pos).mag();
float radii = p1.radius + p2.radius;
if (dist <= radii) {
contacts.add(new ParticleContact(p1, p2));
}
}
}
return contacts;
}
void resolveContacts(ArrayList contacts, float dt) {
int maxIterations = 100;
int iterationsUsed = 0;
int maxIndex = 0;
while (iterationsUsed < maxIterations) {
float max = 0;
for (int i =0; i < contacts.size(); i++) {
float sepVel = contacts.get(i).calculateSeparatingVelocity();
// Confusingly we are using < because separating velocities are
// negative.
if (sepVel < max) {
max = sepVel;
maxIndex = i;
}
}
contacts.get(maxIndex).resolve(dt);
iterationsUsed++;
}
}
void resolveAllParticleContacts(ArrayList allParticles, float dt) {
resolveContacts(
checkForParticleCollisions(allParticles),
dt
);
}
It may look like it, but this is not a proper rigid sphere simulator (as I had erroneously written in the old notes).
That is because it does not at all simulate rotation of the spheres, which is of course unrealistic.
Axis Angle Notation
This is (to me) the most intuitive notation for a rotation. You give a vector representing an axis, and then an angle,
and we interpret it as being a turn of angle degrees about the axis
$$
\theta \
\begin{bmatrix}
x \\
y \\
z\\
\end{bmatrix}
$$
So a 90 degree turn around the X axis is like this:
$$
90 \
\begin{bmatrix}
1 \\
0 \\
0\\
\end{bmatrix}
$$
I'm not going to elaborate much further because I think this is the intutive and sane way to store angles.
It is then unfortunate that you cannot get very far in the arcane arts while staying fully sane, and we must
now open the dark tomb of ring theory.
Quaternions
Let's do a quick review of these. Please keep in mind that as with most of my work, I have no idea
what I am doing.
A quaternion is a method of representing 3D rotations using 4 numbers. To do these we make use of 3 different
imaginary numbers, $i$, $j$, and $k$. All quaternions are just a weighted sum of these bad boys.
$$
w + xi + yj + zk
$$
Mathematicians like to be tricky though, this is because they are a cruel and unforgiving people. In
order to write this simple weighted sum out in a more elaborate tricky way, we use the following notation
$$
\begin{bmatrix}
cos(\frac{\theta}{2}) \\
x sin(\frac{\theta}{2}) \\
y sin(\frac{\theta}{2}) \\
z sin(\frac{\theta}{2}) \\
\end{bmatrix}
$$
This describes some amount of rotation $\theta$ about some axis defined by $x, y, z$, similar to axis-angle notation.
We can combine quaternions by multiplying them, so for example:
$$(w_1 + x_1i + y_1j + z_1k) \times (w_2 + x_2i + y_2j + z_2k) = $$
$$ (w_1w_2 - x_1x_2 - y_1y_2 - z_1z_2) +$$
$$ (w_1x_2 + x_1w_2 - y_1z_2 - z_1y_2) i + $$
$$ (w_1y_2 - x_2z_2 + y_1w_2 - z_1x_2) j + $$
$$ (w_1z_2 + x_1y_2 - y_1x_2 + z_1w_2)k $$
That's a bit of a mouthful though, so we'll stick with the matrix format. If a quaternion is properly formed
then it's magnitude should always be 1. Like this:
$$
\sqrt{ w^2 + x^2 + y^2 + z^2} = 1
$$
If we have a fucked up quaternion and we would like to un-fuck it, we
can normalize it just like a vector.
We're going to use a lot of different notations for storing angles, so to stop it from getting confusing, I'll
put a little q underneath a variable when it's a quaternion.
$$
\mathop{\Theta}_{q} = \text{I'm a quaternion!}
$$
Angular Velocities
The angular velocity of an object represents not the current orientation of an object, but
about what axis, and how quickly, that object is rotating. So naturally, we can store it as an axis in 3D
and an angle (in radians), like so
$$
\dot\theta = r \hat{a}
$$
Where of course $r$ is the rotation, and $\vec{a}$ is the axis (a 3 dimensional vector), so for example a rotation
of 90 degrees along the X-axis would look like this
$$
\dot\theta = \frac{\pi}{2}
\begin{bmatrix}
1 \\
0 \\
0 \\
\end{bmatrix}
$$
Of course vector-scaler multiplication is far from difficult, so we could have written that out like this
$$
\dot\theta = \begin{bmatrix} \frac{\pi}{2} \\ 0 \\ 0 \end{bmatrix}
$$
But let's not do that, because if we have to change the angle later we'll have to change it in 3 places, and I
can't be bothered to re-write it. The same goes for when it's inside the computer, if He has to change it in three
places it will annoy Him.
This format is nice because it means we can add angular velocities together just like adding vectors
$$
\dot\theta^\prime = \dot\theta_1 + \dot\theta_2
$$
Then if we want to do time-step integration with our angular velocity, we need to update our orientation.
We decided earlier to store orientations with quaternions though. So to update an orientation (quaternion)
by an angular velocity (vector), with respect to time (scalar), we use this equation:
$$
\mathop{\theta}_{q}^\prime = \mathop{\theta}_{q} + \frac{\Delta t}{2} \mathop{\omega}_{q} + \mathop{\theta}_{q}
$$
where
$$
\mathop{\omega}_{q} = \begin{bmatrix} 0 \\ \dot\theta_x \\ \dot\theta_y \\ \dot\theta_z \end{bmatrix}
$$
So just to summarize, we have decided to store the rate of change of a rotation as a 3 dimensional vector,
but the current orientation of the object as a quaternion, and our reasons for doing so mostly have to do
with trying to minimize the number of operations required to update the current orientation, or to add
angular velocities.
Angular Acceleration
Angular acceleration is just the derivative of angular velocity. We're storing angular velocity as a 3 dimensional
vector, so this is pretty easy.
$$
\dot\theta^\prime = \dot\theta + \ddot\theta t
$$
Torque and Inertia
Normally we have that Newton equation, y'know the one it's all like:
$$
\ddot{p} = m^{-1}\vec{f}
$$
When we rotate stuff it's a bit like that. Except instead of mass we have inertia, and instead of a force
vector we have a torque vector.
$$
\ddot{\theta} = I^{-1}\vec{\tau}
$$
Where $\ddot{\theta}$ is the rotational acceleration, $\vec{\tau}$ is the torque, and $I^{-1}$ is the inverse of
the inertia matrix. Guess I have to explain those things now too.
Torque
Torque is like force but more twisty, think of trying to turn a screwdriver to put in a screw. The force you
put on the screwdriver is called torque. In fact, this is where we get the name "torque", the twisting force
is named after the annoying screws Chinese factories put on stuff they don't want you to fix.
Because the term "torque" is copyrighted by The Torque Screw Manufacturing Company some people prefer to use
the trademark free term "moment". I don't believe in intelectual property so I won't bother.
Force and torque are linked by this equation:
$$
\vec{\tau} = \vec{p_f} \times \vec{f}
$$
Where $\vec{\tau}$ and $\vec{f}$ are the same thing they were 20 seconds ago, and $\vec{p_f}$ is the coordinates
of the point the force is being applied to, relative to the center of mass of the object.
If you poke an object directly at it's center of mass, you won't put any torque on it. If you don't do that
it'll rotate around an axis. That means we have to write the axis down somewhere, so we'll just put it next to
the magnitude.
$$
\vec{\tau} = a\hat{d}
$$
If you can't remember the other notes (because you didn't read them), the $\hat{ }$ indicates we should
normalize the axis.
Inertia
When you move particles, they like to keep moving.
If you want them to stop you have to hit them with something.
This is more important when you remember that stuff is made of particles, so when you rotate (move) the particle
in the stuff they'll want to keep rotating. If the particles are connected, which they usually are in stuff, and
they all weight the same amount, then the total inertia (how hard it is to stop them from rotating) is the sum
of all their resistance to acceleration (which we call mass).
$$
I_a = \sum^{n}_{i=1}{m_id^2_{p_i \to a}}
$$
Where $d_{p_i \to a}$ is the distance from the particle to the axis, $a$ is the same thing it was a moment ago, and
$I_a$ is the moment of inertia around that axis.
We store inertia in a matrix, which some people like to call the inertia matrix. Because this term is easy to
understand and intuitive, I will avoid using it- and instead use the more annoying complicated term inertia tensor.
They look like this
$$
\begin{bmatrix}
I_x & -I_{xy} & -I_{xz} \\
-I_{xy} & I_y & -I_{xz} \\
-I_{xz} & -I_{yz} & I_z \\
\end{bmatrix}
$$
Where $I_{whatever}$ is the inertia of that object about its $whatever$ axis, through its center of mass.
The rest of the rows are called products of inertia, and the are defined with this:
$$
I_{xy} = \sum^{n}_{i=1} m_i ( \vec{p_i} \cdot \vec{x}) ( \vec{p_i} \cdot \vec{y})
$$
Where $\vec{p_i}$ is the position of each particle relative to the center of mass, and $\vec{x}$ is the X axis.
So $\vec{p_i} \cdot \vec{x}$ is the X component of the distance of particle $i$ to the center of mass.
Computing these inertia tensors is a bit annoying, so we'll often steal them off of Wikipedia. They are consistent
for rigid bodies that are the same shape and material.
Adding Torque Together
Like we mentioned above, torque is just a funny expression of force, and forces can be added together
so in theory we should be able to add together torques somehow too. Actually, it's just like forces:
$$
\tau = \sum_{i}{\tau_i}
$$
Matrix Formats
We only really need $3\times3$ and $3\times4$ matrices, so that's all we'll implement. It's possible (actually
it's even easy) to make a general matrix class that can support arbitrary sizes, but supporting arbitrary sizes
is slow, and physiscs engines should be fast. So we'll only implement $3\times3$ and $3\times4$ matrices. Actually,
we won't really implement the $3\times4$ matrix properly, you'll see.
Multiplication
Multiplication in our $3\times3$ matrices will be totally normal, but for our $3\times4$ matrices let's make it
a bit fucky. See, we only want the 4th row of our $3\times4$ matrices for homogenous coordinates, and by definition
the last row of a set of homogenous coordinate is always $\begin{bmatrix} 0 & 0 & 1 \end{bmatrix}$. Let's not
waste bytes storing the redundant part, and instead we'll just assume that the last row is always $\begin{bmatrix} 0
& 0 & 1 \end{bmatrix}$.
See, this is where things get a bit screwy. We'll refer to our Matrix4 class as a $3\times4$ matrix
(because that's what it's storing), but often we want to use it as a $4\times4$ class (using that extra redundant
row). That means our Matrix4 class gets a $4\times4$ by $4\times4$ matrix multiplication function.
But if you multiply two $4\times4$ matrices with $\begin{bmatrix} 0
& 0 & 1 \end{bmatrix}$, as the bottom row, you just get a another $4\times4$ matrix with $\begin{bmatrix} 0
& 0 & 1 \end{bmatrix}$ as the bottom row. So we won't even do the arithmetic for the last row, and we'll just
shove it on at the end. So our Matrix4's $4\times4$ multiplication function doesn't actually
do $4\times4$ matrix multiplication or $3\times4$ matrix multplication, it does some homogeneous coordinate
operation that's similar.
Finding The Inverse
Finding the inverse of a $3\times3$ matrix is fortunately pretty simple, given a matrix $M$ as follows:
$$
M = \begin{bmatrix}
a & b & c \\
d & e & f \\
g & h & i
\end{bmatrix}
$$
Then we can compute it's inverse as:
$$
M^{-1} = \frac{1}{\text{det}M} \begin{bmatrix}
ei-fh & ch-bi & bf-ce \\
fg-di & ai-cg & cd - af \\
dh-eg & bg-ah & ae - bd
\end{bmatrix}
$$
Where:
$$
\text{det}M = aei + dhc + gbf -ahf - gec - dbi
$$
Finding The Transpose
Why'd I make this section this is like the easiest operation in linear algebra.
Whatever, to find the transpose of a matrix you just flip it over $y=x$. It looks like this:
$$
\begin{bmatrix}
a & b \\
c & d
\end{bmatrix}^T
=
\begin{bmatrix}
a & c \\
b & d
\end{bmatrix}
$$
Code
class Matrix3 {
float data[] = new float[9];
public Matrix3() {
for (int i =0; i < data.length; i++) data[i] = 0;
}
public Matrix3(float[] d) {
this.data = d;
}
void invert() {
setInverse(this);
}
Matrix3 getInverse() {
Matrix3 r = new Matrix3();
r.setInverse(this);
return r;
}
void setInverse(Matrix3 m) {
// Calculate the determinant and make sure it isn't 0.
float t4 = m.data[0] * m.data[4];
float t6 = m.data[0]*m.data[5];
float t8 = m.data[1]*m.data[3];
float t10 = m.data[2]*m.data[4];
float t12 = m.data[1]*m.data[6];
float t14 = m.data[2]*m.data[6];
float t16 = (t4*m.data[8] - t6*m.data[7] - t8*m.data[8] + t10*m.data[7] + t12*m.data[5] - t14*m.data[4]);
assert(t16 != 0.0); // if it is 0 shit our pants and cry.
float t17 = 1/t16; // we're gonna be dividing a lot of stuff by this
// Then we can calculate the actual inverse.
data[0] = (m.data[4]*m.data[8]-m.data[5]*m.data[7])*t17;
data[1] = -(m.data[1]*m.data[8]-m.data[2]*m.data[7])*t17;
data[2] = (m.data[1]*m.data[5]-m.data[2]*m.data[4])*t17;
data[3] = -(m.data[3]*m.data[8]-m.data[5]*m.data[6])*t17;
data[4] = (m.data[0]*m.data[8]-t14)*t17;
data[5] = -(t6-t10)*t17;
data[6] = (m.data[3]*m.data[7]-m.data[4]*m.data[6])*t17;
data[7] = -(m.data[0]*m.data[7]-t12)*t17;
data[8] = (t4-t8)*t17;
}
Matrix3 getTranspose() {
Matrix3 m = new Matrix3();
m.setTranspose(this);
return m;
}
void setTranspose(Matrix3 m) {
data[0] = m.data[0];
data[1] = m.data[3];
data[2] = m.data[6];
data[3] = m.data[1];
data[4] = m.data[4];
data[5] = m.data[7];
data[6] = m.data[2];
data[7] = m.data[5];
data[8] = m.data[8];
}
Matrix3 mult(Matrix3 o) {
float d2[] = new float[9];
d2[0] = data[0]*o.data[0] + data[1]*o.data[3] + data[2]*o.data[6];
d2[1] = data[0]*o.data[1] + data[1]*o.data[4] + data[2]*o.data[7];
d2[2] = data[0]*o.data[2] + data[1]*o.data[5] + data[2]*o.data[8];
d2[3] = data[3]*o.data[0] + data[4]*o.data[3] + data[5]*o.data[6];
d2[4] = data[3]*o.data[1] + data[4]*o.data[4] + data[5]*o.data[7];
d2[5] = data[3]*o.data[2] + data[4]*o.data[5] + data[5]*o.data[8];
d2[6] = data[6]*o.data[0] + data[7]*o.data[3] + data[8]*o.data[6];
d2[7] = data[6]*o.data[1] + data[7]*o.data[4] + data[8]*o.data[7];
d2[8] = data[6]*o.data[2] + data[7]*o.data[5] + data[8]*o.data[8];
Matrix3 f = new Matrix3(d2);
return f;
}
}
class Matrix4 {
float data[] = new float[12];
public Matrix4() {
for (int i =0; i < data.length; i++) data[i] = 0;
}
public Matrix4(float[] d) {
this.data = d;
}
PVector mult(PVector vec) {
return new PVector (
vec.x*data[0] + vec.y*data[1] + vec.z*data[2] + data[3],
vec.x*data[4] + vec.y*data[5] + vec.z*data[6] + data[7],
vec.x*data[8] + vec.y*data[9] + vec.z*data[10] + data[11]
);
}
Matrix4 mult(Matrix4 o) {
float[] result = new float[12];
data[0] = (o.data[0]*data[0]) + (o.data[4]*data[1]) + (o.data[8]*data[2]);
data[4] = (o.data[0]*data[4]) + (o.data[4]*data[5]) + (o.data[8]*data[6]);
data[8] = (o.data[0]*data[8]) + (o.data[4]*data[9]) + (o.data[8]*data[10]);
data[1] = (o.data[1]*data[0]) + (o.data[5]*data[1]) + (o.data[9]*data[2]);
data[5] = (o.data[1]*data[4]) + (o.data[5]*data[5]) + (o.data[9]*data[6]);
data[9] = (o.data[1]*data[8]) + (o.data[5]*data[9]) + (o.data[9]*data[10]);
data[2] = (o.data[2]*data[0]) + (o.data[6]*data[1]) + (o.data[10]*data[2]);
data[6] = (o.data[2]*data[4]) + (o.data[6]*data[5]) + (o.data[10]*data[6]);
data[10] = (o.data[2]*data[8]) + (o.data[6]*data[9]) + (o.data[10]*data[10]);
data[3] = (o.data[3]*data[0]) + (o.data[7]*data[1]) + (o.data[11]*data[2]) + data[3];
data[7] = (o.data[3]*data[4]) + (o.data[7]*data[5]) + (o.data[11]*data[6]) + data[7];
data[11] = (o.data[3]*data[8]) + (o.data[7]*data[9]) + (o.data[11]*data[10]) + data[11];
return new Matrix4(result);
}
float getDeterminant() {
return data[8]*data[5]*data[2]+
data[4]*data[9]*data[2]+
data[8]*data[1]*data[6]-
data[0]*data[9]*data[6]-
data[4]*data[1]*data[10]+
data[0]*data[5]*data[10];
}
void setInverse(Matrix4 m) {
// Calculate if the determinant is 0 and if it is shit out pants
float det = getDeterminant();
assert(det != 0.0);
// I shamelessly copy-pasted this from the book because I cannot be asked to type it all out.
// Don't sue me or I will cry.
det = (1.0)/det;
data[0] = (-m.data[9]*m.data[6]+m.data[5]*m.data[10])*det;
data[4] = (m.data[8]*m.data[6]-m.data[4]*m.data[10])*det;
data[8] = (-m.data[8]*m.data[5]+m.data[4]*m.data[9]* m.data[15])*det;
data[1] = (m.data[9]*m.data[2]-m.data[1]*m.data[10])*det;
data[5] = (-m.data[8]*m.data[2]+m.data[0]*m.data[10])*det;
data[9] = (m.data[8]*m.data[1]-m.data[0]*m.data[9]* m.data[15])*det;
data[2] = (-m.data[5]*m.data[2]+m.data[1]*m.data[6]* m.data[15])*det;
data[6] = (+m.data[4]*m.data[2]-m.data[0]*m.data[6]* m.data[15])*det;
data[10] = (-m.data[4]*m.data[1]+m.data[0]*m.data[5]* m.data[15])*det;
data[3] = (m.data[9]*m.data[6]*m.data[3] -m.data[5]*m.data[10]*m.data[3]
-m.data[9]*m.data[2]*m.data[7]
+m.data[1]*m.data[10]*m.data[7]
+m.data[5]*m.data[2]*m.data[11]
-m.data[1]*m.data[6]*m.data[11])*det;
data[7] = (-m.data[8]*m.data[6]*m.data[3]
+m.data[4]*m.data[10]*m.data[3]
+m.data[8]*m.data[2]*m.data[7]
-m.data[0]*m.data[10]*m.data[7]
-m.data[4]*m.data[2]*m.data[11]
+m.data[0]*m.data[6]*m.data[11])*det;
data[11] =(m.data[8]*m.data[5]*m.data[3]
-m.data[4]*m.data[9]*m.data[3]
-m.data[8]*m.data[1]*m.data[7]
+m.data[0]*m.data[9]*m.data[7]
+m.data[4]*m.data[1]*m.data[11]
-m.data[0]*m.data[5]*m.data[11])*det;
}
}